﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

using System.IO;
using AForge.Robotics.Lego;

namespace nxt
{
    class Control
    {

        //public int topApos, topBpos, botApos, botBpos;

        NXTBrick frontModul = new NXTBrick(); //felső
        NXTBrick backModul = new NXTBrick(); //alsó
        
        string comPort1;
        string comPort2;

        public NXTBrick FrontModul
        { get { return frontModul; } }

        

        public NXTBrick BackModul
        { get { return backModul; } }

        #region CONNECT

        public Control(string _comPort1, string _comPort2)
        {
            comPort1 = _comPort1;
            comPort2 = _comPort2;
        }
                
        public void Connect()
        {
            frontModul.Connect(comPort1);
            backModul.Connect(comPort2);
        }

        /// <summary>
        /// felső nxt
        /// </summary>
        /// <returns></returns>
        public bool nxtConnectionStatusFront()
        {
            bool status = false;

            return status = frontModul.IsConnected;
        }

        /// <summary>
        /// alsó nxt
        /// </summary>
        /// <returns></returns>
        public bool nxtConnectionStatusBack()
        {
            bool status = false;

            return status = backModul.IsConnected;
        }

        public void Disconnect()
        {

            try
            {
                backModul.Disconnect();
                frontModul.Disconnect();
            }
            catch
            {
                //
            }
        }

        #endregion

        #region LIGHT

        public void greenLight()
        { FrontModul.SetSensorMode(NXTBrick.Sensor.First, NXTBrick.SensorType.ColorGreen, NXTBrick.SensorMode.Raw); }

        public void redLight()
        { FrontModul.SetSensorMode(NXTBrick.Sensor.First, NXTBrick.SensorType.ColorRed, NXTBrick.SensorMode.Raw); }

        #endregion

        #region SIMPLE CONTROL

        /// <summary>
        /// motor paramétereinek megadása
        /// </summary>
        /// <param name="_speed"></param>
        /// <returns></returns>
        public NXTBrick.MotorState setMotorState(int _power, int _turnRatio, int _tachoLimit)
        {
            NXTBrick.MotorState motorState = new NXTBrick.MotorState();


            motorState.Power = _power; // [-100,100]
            motorState.TurnRatio = _turnRatio;  //??? Turn ratio to use for a motor that is 'slaved' to another motor. +100 is same as primary motor. Zero is stopped. -100 is same movement as primary but in inverted directon. Values between 100 and 0 give porportional power levels from the primary motor to the slave motor.
            motorState.Mode = NXTBrick.MotorMode.On;
            motorState.Regulation = NXTBrick.MotorRegulationMode.Sync;
            motorState.RunState = NXTBrick.MotorRunState.Running;
            motorState.TachoLimit = _tachoLimit; //fordulat fokban
            

            return motorState;
        }

        /// <summary>
        /// felső nxt motorok mozgatása
        /// </summary>
        /// <param name="_motor"></param>
        /// <param name="_power"></param>
        /// <param name="_tacho"></param>
        public void moveFrontMotor(NXTBrick.Motor _motor, int _power, int _tacho)
        {
            frontModul.SetMotorState(_motor, setMotorState(_power, 100, _tacho));
        }

        /// <summary>
        /// alsó nxt motorok mozgatása
        /// </summary>
        /// <param name="_motor"></param>
        /// <param name="_power"></param>
        /// <param name="_tacho"></param>
        public void moveBackMotor(NXTBrick.Motor _motor, int _power, int _tacho)
        {
            backModul.SetMotorState(_motor, setMotorState(_power, 100, _tacho));
        }

        /// <summary>
        /// előre
        /// </summary>
        public void forward(int _power, int _turnRatio, int _tachoLimit)
        {
            frontModul.SetMotorState(NXTBrick.Motor.A, setMotorState(_power, _turnRatio, _tachoLimit));
            frontModul.SetMotorState(NXTBrick.Motor.B, setMotorState(_power, _turnRatio, _tachoLimit));

            backModul.SetMotorState(NXTBrick.Motor.A, setMotorState(-_power, _turnRatio, _tachoLimit));
            backModul.SetMotorState(NXTBrick.Motor.B, setMotorState(-_power, _turnRatio, _tachoLimit));
        }

        /// <summary>
        /// hátra
        /// </summary>
        public void backward(int _power, int _turnRatio, int _tachoLimit)
        {
            frontModul.SetMotorState(NXTBrick.Motor.A, setMotorState(-_power, _turnRatio, _tachoLimit));
            frontModul.SetMotorState(NXTBrick.Motor.B, setMotorState(-_power, _turnRatio, _tachoLimit));

            backModul.SetMotorState(NXTBrick.Motor.A, setMotorState(_power, _turnRatio, _tachoLimit));
            backModul.SetMotorState(NXTBrick.Motor.B, setMotorState(_power, _turnRatio, _tachoLimit));
        }

        /// <summary>
        /// jobbra
        /// </summary>
        public void right(int _power, int _turnRatio, int _tachoLimit)
        {
            frontModul.SetMotorState(NXTBrick.Motor.A, setMotorState(_power, _turnRatio, _tachoLimit));
            frontModul.SetMotorState(NXTBrick.Motor.B, setMotorState(-_power, _turnRatio, _tachoLimit));

            backModul.SetMotorState(NXTBrick.Motor.A, setMotorState(_power, _turnRatio, _tachoLimit));
            backModul.SetMotorState(NXTBrick.Motor.B, setMotorState(-_power, _turnRatio, _tachoLimit));
        }

        /// <summary>
        /// balra
        /// </summary>
        public void left(int _power, int _turnRatio, int _tachoLimit)
        {
            frontModul.SetMotorState(NXTBrick.Motor.A, setMotorState(-_power, _turnRatio, _tachoLimit));
            frontModul.SetMotorState(NXTBrick.Motor.B, setMotorState(_power, _turnRatio, _tachoLimit));

            backModul.SetMotorState(NXTBrick.Motor.A, setMotorState(-_power, _turnRatio, _tachoLimit));
            backModul.SetMotorState(NXTBrick.Motor.B, setMotorState(_power, _turnRatio, _tachoLimit));
        }

        /// <summary>
        /// emelkedik
        /// </summary>
        /// <param name="_power"></param>
        /// <param name="_turnRatio"></param>
        /// <param name="_tachoLimit"></param>
        public void up(int _power, int _turnRatio, int _tachoLimit)
        {
            frontModul.SetMotorState(NXTBrick.Motor.A, setMotorState(_power, _turnRatio, _tachoLimit));
            frontModul.SetMotorState(NXTBrick.Motor.B, setMotorState(_power, _turnRatio, _tachoLimit));

            backModul.SetMotorState(NXTBrick.Motor.A, setMotorState(_power, _turnRatio, _tachoLimit));
            backModul.SetMotorState(NXTBrick.Motor.B, setMotorState(_power, _turnRatio, _tachoLimit));
        }

        /// <summary>
        /// süllyed
        /// </summary>
        /// <param name="_power"></param>
        /// <param name="_turnRatio"></param>
        /// <param name="_tachoLimit"></param>
        public void down(int _power, int _turnRatio, int _tachoLimit)
        {
            frontModul.SetMotorState(NXTBrick.Motor.A, setMotorState(-_power, _turnRatio, _tachoLimit));
            frontModul.SetMotorState(NXTBrick.Motor.B, setMotorState(-_power, _turnRatio, _tachoLimit));

            backModul.SetMotorState(NXTBrick.Motor.A, setMotorState(-_power, _turnRatio, _tachoLimit));
            backModul.SetMotorState(NXTBrick.Motor.B, setMotorState(-_power, _turnRatio, _tachoLimit));
        }

        public void correction(int _powerFA,int _powerFB,int _powerBA,int _powerBB , int _tachoLimitFA, int _tachoLimitFB, int _tachoLimitBA, int _tachoLimitBB)
        {
            frontModul.SetMotorState(NXTBrick.Motor.A, setMotorState(-_powerFA, 100, _tachoLimitFA));
            frontModul.SetMotorState(NXTBrick.Motor.B, setMotorState(-_powerFB, 100, _tachoLimitFB));

            backModul.SetMotorState(NXTBrick.Motor.A, setMotorState(-_powerBA, 100, _tachoLimitBA));
            backModul.SetMotorState(NXTBrick.Motor.B, setMotorState(-_powerBB, 100, _tachoLimitBB));
        }

        #endregion

        #region MOVEMENT CALCULATION

        public int[] countForwardBackwordMovement(double height, double moveX, int topApos, int topBpos, int botApos, int botBpos)
        {
            int[] ret = new int[4];
            //double topApos, topBpos, botApos, botBpos;
            topApos = Convert.ToInt32((Math.Abs(topApos - Math.Sqrt((height * height) + (topApos + moveX) * (topApos + moveX))) / 66.5) * 360);
            topBpos = Convert.ToInt32((Math.Abs(topBpos - Math.Sqrt((height * height) + (topBpos + moveX) * (topBpos + moveX))) / 66.5) * 360);
            botApos = Convert.ToInt32((Math.Abs(botApos - Math.Sqrt((height * height) + (botApos - moveX) * (botApos - moveX))) / 66.5) * 360);
            botBpos = Convert.ToInt32((Math.Abs(botBpos - Math.Sqrt((height * height) + (botBpos - moveX) * (botBpos - moveX))) / 66.5) * 360);
            ret[0] = topApos;
            ret[1] = topBpos;
            ret[2] = botApos;
            ret[3] = botBpos;
            return ret;
            //a motor értéke most már adott fordulatban
        }

        public int[] countSideMovement(double height, double moveY, int topApos, int topBpos, int botApos, int botBpos)
        {
            int[] ret = new int[4];
            //double topApos, topBpos, botApos, botBpos;
            topApos = Convert.ToInt32((Math.Abs(topApos - Math.Sqrt((height * height) + (topApos + moveY) * (topApos + moveY))) / 66.5) * 360);
            topBpos = Convert.ToInt32((Math.Abs(topBpos - Math.Sqrt((height * height) + (topBpos - moveY) * (topBpos - moveY))) / 66.5) * 360);
            botApos = Convert.ToInt32((Math.Abs(botApos - Math.Sqrt((height * height) + (botApos + moveY) * (botApos + moveY))) / 66.5) * 360);
            botBpos = Convert.ToInt32((Math.Abs(botBpos - Math.Sqrt((height * height) + (botBpos - moveY) * (botBpos - moveY))) / 66.5) * 360);
            ret[0] = topApos;
            ret[1] = topBpos;
            ret[2] = botApos;
            ret[3] = botBpos;
            return ret;
            //a motor értéke most már adott fordulatban
        }

        public int[] countUpDownMovement(double height, double moveZ, int topApos, int topBpos, int botApos, int botBpos)
        {
            int[] ret = new int[4];
            //double topApos, topBpos, botApos, botBpos;
            topApos = Convert.ToInt32((Math.Abs(topApos - Math.Sqrt((height + moveZ) * (height + moveZ) + (topApos * topApos))) / 66.5) * 360);
            topBpos = Convert.ToInt32((Math.Abs(topBpos - Math.Sqrt((height + moveZ) * (height + moveZ) + (topBpos * topBpos))) / 66.5) * 360);
            botApos = Convert.ToInt32((Math.Abs(botApos - Math.Sqrt((height + moveZ) * (height + moveZ) + (botApos * botApos))) / 66.5) * 360);
            botBpos = Convert.ToInt32((Math.Abs(botBpos - Math.Sqrt((height + moveZ) * (height + moveZ) + (botBpos * botBpos))) / 66.5) * 360);
            ret[0] = topApos;
            ret[1] = topBpos;
            ret[2] = botApos;
            ret[3] = botBpos;
            return ret;
            //a motor értéke most már adott fordulatban
        }

        /// <summary>
        /// sleep
        /// </summary>
        /// <param name="_mm"></param>
        /// <returns></returns>
        

        #endregion

        public int getSleep(int _mm)
        {
            return _mm * 50;
        }

        #region MOVEMENT

        /// <summary>
        /// előre
        /// </summary>
        public void forward(int _power, int _turnRatio, int _tachoTopA, int _tachoTopB, int _tachoBotA, int _tachoBotB)
        {
            frontModul.SetMotorState(NXTBrick.Motor.A, setMotorState(_power, _turnRatio, _tachoTopA));
            frontModul.SetMotorState(NXTBrick.Motor.B, setMotorState(_power, _turnRatio, _tachoTopB));

            backModul.SetMotorState(NXTBrick.Motor.A, setMotorState(-_power, _turnRatio, _tachoBotA));
            backModul.SetMotorState(NXTBrick.Motor.B, setMotorState(-_power, _turnRatio, _tachoBotB));
        }

        /// <summary>
        /// hátra
        /// </summary>
        public void backward(int _power, int _turnRatio, int _tachoTopA, int _tachoTopB, int _tachoBotA, int _tachoBotB)
        {
            frontModul.SetMotorState(NXTBrick.Motor.A, setMotorState(-_power, _turnRatio, _tachoTopA));
            frontModul.SetMotorState(NXTBrick.Motor.B, setMotorState(-_power, _turnRatio, _tachoTopB));

            backModul.SetMotorState(NXTBrick.Motor.A, setMotorState(_power, _turnRatio, _tachoBotA));
            backModul.SetMotorState(NXTBrick.Motor.B, setMotorState(_power, _turnRatio, _tachoBotB));
        }

        /// <summary>
        /// jobbra
        /// </summary>
        public void right(int _power, int _turnRatio, int _tachoTopA, int _tachoTopB, int _tachoBotA, int _tachoBotB)
        {
            frontModul.SetMotorState(NXTBrick.Motor.A, setMotorState(_power, _turnRatio, _tachoTopA));
            frontModul.SetMotorState(NXTBrick.Motor.B, setMotorState(-_power, _turnRatio, _tachoTopB));

            backModul.SetMotorState(NXTBrick.Motor.A, setMotorState(_power, _turnRatio, _tachoBotA));
            backModul.SetMotorState(NXTBrick.Motor.B, setMotorState(-_power, _turnRatio, _tachoBotB));
        }

        /// <summary>
        /// balra
        /// </summary>
        public void left(int _power, int _turnRatio, int _tachoTopA, int _tachoTopB, int _tachoBotA, int _tachoBotB)
        {
            frontModul.SetMotorState(NXTBrick.Motor.A, setMotorState(-_power, _turnRatio, _tachoTopA));
            frontModul.SetMotorState(NXTBrick.Motor.B, setMotorState(_power, _turnRatio, _tachoTopB));

            backModul.SetMotorState(NXTBrick.Motor.A, setMotorState(-_power, _turnRatio, _tachoBotA));
            backModul.SetMotorState(NXTBrick.Motor.B, setMotorState(_power, _turnRatio, _tachoBotB));
        }

        /// <summary>
        /// emelkedik
        /// </summary>
        /// <param name="_power"></param>
        /// <param name="_turnRatio"></param>
        /// <param name="_tachoLimit"></param>
        public void up(int _power, int _turnRatio, int _tachoTopA, int _tachoTopB, int _tachoBotA, int _tachoBotB)
        {
            frontModul.SetMotorState(NXTBrick.Motor.A, setMotorState(_power, _turnRatio, _tachoTopA));
            frontModul.SetMotorState(NXTBrick.Motor.B, setMotorState(_power, _turnRatio, _tachoTopB));

            backModul.SetMotorState(NXTBrick.Motor.A, setMotorState(_power, _turnRatio, _tachoBotA));
            backModul.SetMotorState(NXTBrick.Motor.B, setMotorState(_power, _turnRatio, _tachoBotB));
        }

        /// <summary>
        /// süllyed
        /// </summary>
        /// <param name="_power"></param>
        /// <param name="_turnRatio"></param>
        /// <param name="_tachoLimit"></param>
        public void down(int _power, int _turnRatio, int _tachoTopA, int _tachoTopB, int _tachoBotA, int _tachoBotB)
        {
            frontModul.SetMotorState(NXTBrick.Motor.A, setMotorState(-_power, _turnRatio, _tachoTopA));
            frontModul.SetMotorState(NXTBrick.Motor.B, setMotorState(-_power, _turnRatio, _tachoTopB));

            backModul.SetMotorState(NXTBrick.Motor.A, setMotorState(-_power, _turnRatio, _tachoBotA));
            backModul.SetMotorState(NXTBrick.Motor.B, setMotorState(-_power, _turnRatio, _tachoBotB));
        }

        /// <summary>
        /// Robotic arm close and open
        /// </summary>
        /// <param name="close"></param>
        /// <returns></returns>
        public bool robotic_arm_close_open(bool close)
        {
            if (close == false)
            {
                backModul.SetMotorState(NXTBrick.Motor.C, setMotorState(10, 100, 68));
                close = true;
            }
            else
            {
                backModul.SetMotorState(NXTBrick.Motor.C, setMotorState(-10, 100, 68));
                close = false;
            }
            return close;
        }


        #endregion

        #region CALC HMáté

        int tachoTopA;
        int tachoTopB;
        int tachoBotA;
        int tachoBotB;

        double lengthForward;
        double lengthBackward;
        double lengthRight;
        double lengthLeft;

        double sumTopA;
        double sumTopB;
        double sumBotA;
        double sumBotB;


        double height; //magasság a motoroktól a tetejéig

        //átmenetei változók, permSum = a damil új hossza
        double perm;
        double permSum;
        double permHeight;
        

        //kezdőértékek beállítása, robot középen
        public void setValues()
        {
            //kiengedett damil hossz
            sumTopA = 131;
            sumTopB = 131;
            sumBotA = 131;
            sumBotB = 131;

            //robot távolsága előre, hátra, balra, jobbra
            lengthForward = 115.75;
            lengthBackward = 115.75;
            lengthLeft = 56.75;
            lengthRight = 56.75;

            height = 121 - 61.5; //121 oszlop magasság, 61,5 lentről való magassága a motoroknak
        }

        //tacho értékek nullázása
        private void setTachoNull()
        {
            tachoTopA = 0;
            tachoTopB = 0;
            tachoBotA = 0;
            tachoBotB = 0;
        }

        //egy motor tachoLimit kiszámítása, előre-hátra, jobbra-balra
        private int calc(double _lengthBwFw, double _lengthRtLt, double _sum)
        {
            perm = 0;
            permSum = 0;

            perm = Math.Sqrt(_lengthBwFw * _lengthBwFw + _lengthRtLt * _lengthRtLt);
            permSum = Math.Sqrt(height * height + perm * perm);



            //különbség nagyobb nulla, + tacho
            if (_sum - permSum > 0)
            { return Convert.ToInt32(Math.Round(((_sum - permSum) / 6.4 * 360), 0)); }
            //különbség kisebb nulla, - tacho
            else
            { return Convert.ToInt32(Math.Round(((_sum - permSum) / 6.4 * 360), 0)); }
        }

        private int calcHeight(double _lengthBwFw, double _lengthRtLt, double _length, double _sum)
        {
            perm = 0;
            permHeight = 0;

            perm = Math.Sqrt(_lengthBwFw * _lengthBwFw + _lengthRtLt * _lengthRtLt);
            permSum = Math.Sqrt((height + _length) * (height + _length) + perm * perm);

            if (_sum - permSum>0)
            { return Convert.ToInt32(Math.Round(((_sum - permSum) / 6.4 * 360), 0)); }
            else
            { return Convert.ToInt32(Math.Round(((_sum - permSum) / 6.4 * 360), 0)); }
        }


        //tachoLimit kiszámítása előre, hátra
        public void calcTachoFwBw(double _length)
        {

            setTachoNull();

            //előre
            if (_length > 0)
            {

                tachoTopA = calc(lengthForward - _length, lengthRight, sumTopA);
                sumTopA = permSum;

                tachoTopB = calc(lengthForward - _length, lengthLeft, sumTopB);
                sumTopB = permSum;

                tachoBotA = calc(lengthBackward + _length, lengthRight, sumBotA);
                sumBotA = permSum;

                tachoBotB = calc(lengthBackward + _length, lengthLeft, sumBotB);
                sumBotB = permSum;

                forward(20, 100, tachoTopA, tachoTopB, tachoBotA, tachoBotB);

                lengthForward -= _length;
                lengthBackward += _length;

            }
            //hátra
            else
            {
                tachoTopA = calc(lengthForward + _length, lengthRight, sumTopA);
                sumTopA = permSum;

                tachoTopB = calc(lengthForward + _length, lengthLeft, sumTopB);
                sumTopB = permSum;

                tachoBotA = calc(lengthBackward - _length, lengthRight, sumBotA);
                sumBotA = permSum;

                tachoBotB = calc(lengthBackward - _length, lengthLeft, sumBotB);
                sumBotB = permSum;

                backward(20, 100, tachoTopA, tachoTopB, tachoBotA, tachoBotB);

                lengthForward += _length;
                lengthBackward -= _length;
            }

            getSleep(Convert.ToInt32(_length*10));
            
        }

        public void calcTachoRightLeft(double _length)
        {
            setTachoNull();

            //jobbra
            if (_length > 0)
            {
                tachoTopA = calc(lengthForward, lengthRight - _length, sumTopA);
                sumTopA = permSum;

                tachoTopB = calc(lengthForward, lengthLeft + _length, sumTopB);
                sumTopB = permSum;

                tachoBotA = calc(lengthBackward, lengthRight - _length, sumBotA);
                sumBotA = permSum;

                tachoBotB = calc(lengthBackward, lengthLeft + _length, sumBotB);
                sumBotB = permSum;

                right(20, 100, tachoTopA, tachoTopB, tachoBotA, tachoBotB);

                lengthLeft += _length;
                lengthRight -= _length;
            }
            //balra
            else
            {
                tachoTopA = calc(lengthForward, lengthRight + _length, sumTopA);
                sumTopA = permSum;

                tachoTopB = calc(lengthForward, lengthLeft - _length, sumTopB);
                sumTopB = permSum;

                tachoBotA = calc(lengthBackward, lengthRight + _length, sumBotA);
                sumBotA = permSum;

                tachoBotB = calc(lengthBackward, lengthLeft - _length, sumBotB);
                sumBotB = permSum;

                right(20, 100, tachoTopA, tachoTopB, tachoBotA, tachoBotB);

                lengthLeft -= _length;
                lengthRight += _length;
            }

            getSleep(Convert.ToInt32(_length*10));
        }

        public void calcTachoUpDown(double _length)
        {
            setTachoNull();

            //fel
            if (_length > 0)
            {
                tachoTopA = calcHeight(lengthForward, lengthRight, _length, sumTopA);
                sumTopA = permSum;

                tachoTopB = calcHeight(lengthForward, lengthLeft,_length,  sumTopB);
                sumTopB = permSum;

                tachoBotA = calcHeight(lengthBackward, lengthRight,_length, sumBotA);
                sumBotA = permSum;

                tachoBotB = calcHeight(lengthBackward, lengthLeft, _length, sumBotB);
                sumBotB = permSum;

                up(20, 100, tachoTopA, tachoTopB, tachoBotA, tachoBotB);

                height -= _length;
            }
            //le
            else
            {
                tachoTopA = calcHeight(lengthForward, lengthRight, _length, sumTopA);
                sumTopA = permSum;

                tachoTopB = calcHeight(lengthForward, lengthLeft, _length, sumTopB);
                sumTopB = permSum;

                tachoBotA = calcHeight(lengthBackward, lengthRight, _length, sumBotA);
                sumBotA = permSum;

                tachoBotB = calcHeight(lengthBackward, lengthLeft, _length, sumBotB);
                sumBotB = permSum;

                down(20, 100, tachoTopA, tachoTopB, tachoBotA, tachoBotB);

                height += _length;
            }

        }



        #endregion

    }
}
